﻿using System;
using GeoAPI.Geometries;

namespace YJKGridTopologyCal.Index.KdTree
{
	public static class KdTreeExtensions
	{
		public static KdNode<T> NearestNeighbor<T>(this KdTree<T> self, Coordinate coord) where T : class
		{
			KdNode<T> result = null;
			double maxValue = double.MaxValue;
			KdTreeExtensions.NearestNeighbor<T>(self.Root, coord, ref result, ref maxValue, true);
			return result;
		}

		private static void NearestNeighbor<T>(KdNode<T> currentNode, Coordinate queryCoordinate, ref KdNode<T> closestNode, ref double closestDistanceSq, bool isOddLevel) where T : class
		{
			while (currentNode != null)
			{
				double num = Math.Pow(currentNode.X - queryCoordinate.X, 2.0) + Math.Pow(currentNode.Y - queryCoordinate.Y, 2.0);
				if (num < closestDistanceSq)
				{
					closestNode = currentNode;
					closestDistanceSq = num;
				}
				KdNode<T> kdNode = currentNode.Left;
				KdNode<T> kdNode2 = currentNode.Right;
				if (currentNode.Left != null && (isOddLevel ? (queryCoordinate.X >= currentNode.X) : (queryCoordinate.Y >= currentNode.Y)))
				{
					kdNode = currentNode.Right;
					kdNode2 = currentNode.Left;
				}
				if (kdNode != null && KdTreeExtensions.NeedsToBeSearched<T>(queryCoordinate, currentNode, closestDistanceSq, kdNode == currentNode.Left, isOddLevel))
				{
					KdTreeExtensions.NearestNeighbor<T>(kdNode, queryCoordinate, ref closestNode, ref closestDistanceSq, !isOddLevel);
				}
				if (kdNode2 == null || !KdTreeExtensions.NeedsToBeSearched<T>(queryCoordinate, currentNode, closestDistanceSq, kdNode2 == currentNode.Left, isOddLevel))
				{
					return;
				}
				currentNode = kdNode2;
				isOddLevel = !isOddLevel;
			}
		}

		private static bool NeedsToBeSearched<T>(Coordinate target, KdNode<T> node, double closestDistSq, bool left, bool isOddLevel) where T : class
		{
			if (isOddLevel)
			{
				return (left ? (target.X <= node.X) : (target.X >= node.X)) || Math.Pow(target.X - node.X, 2.0) < closestDistSq;
			}
			return (left ? (target.Y <= node.Y) : (target.Y >= node.Y)) || Math.Pow(target.Y - node.Y, 2.0) < closestDistSq;
		}
	}
}
